#include "stdafx.h"
#include "Fish.hpp"
#include "Simulation.hpp"
#include "ArtificialSea.h"
#include <boost/lexical_cast.hpp>

int Fish::_id = 0;

//////////////////////////////////////////////////////////////////////////

Fish::Fish(const vec3& pos)
	: Agent(pos)
{
	Ogre::SceneManager* sm = ArtificialSea::getSingleton().getSceneManager();
	Ogre::String name = "Fish" + boost::lexical_cast<Ogre::String>(_id++);
	_sceneNode = sm->getRootSceneNode()->createChildSceneNode(_position);
	_model = sm->createEntity(name, "fish.mesh");
	//_id++;
	_sceneNode->attachObject(_model);
	_sceneNode->resetOrientation();
	_sceneNode->setDirection(_headingSmoothed, Ogre::SceneNode::TS_WORLD);
	_animationState = _model->getAnimationState("swim");
	_animationState->setEnabled(true);
	_animationState->setLoop(true);
}

Fish::~Fish()
{
	_sceneNode->removeAndDestroyAllChildren();
	ArtificialSea::getSingleton().getSceneManager()->destroySceneNode(_sceneNode);
}

//////////////////////////////////////////////////////////////////////////

vec3 Fish::calcSteeringForce(float dt)
{
	vec3 force = vec3::ZERO;

	float weightSum = 
		PM.fish.weights.wander->getRealValue() + PM.fish.weights.separation->getRealValue() + 
		PM.fish.weights.alignment->getRealValue() + PM.fish.weights.cohesion->getRealValue();

	vec3 toDiverVec = getPosition() - ArtificialSea::getSingleton().getDiver()->Agent::getPosition();

	bool predNearby = (Simulation::Get().tagNeighborAgents(*this, PM.fish.evade.dangerRadius->getRealValue(), AGENT_PREDATOR) > 0);
	//bool evadeDiver = (ArtificialSea::getSingleton().getDiver()->getVelocity() != vec3::ZERO && 
	//	toDiverVec.squaredLength() < PM.diver.ranges.evasion->getRealValue()*PM.diver.ranges.evasion->getRealValue());
	bool evadeDiver = false;
	if (predNearby || evadeDiver)
	{
		weightSum += PM.fish.weights.evade->getRealValue();

		vec3 evdForce = vec3::ZERO;
		if (predNearby)
			force += _steering.evade(this, PM.fish.maxVelocity->getRealValue(), PM.fish.evade.turnAround->getRealValue(), PM.fish.evade.timeScale->getRealValue());
		if (evadeDiver)
			force += _steering.evadeAgent(this, ArtificialSea::getSingleton().getDiver(), 
				PM.fish.maxVelocity->getRealValue(), PM.fish.evade.turnAround->getRealValue(), PM.fish.evade.timeScale->getRealValue());

		force += evdForce * (PM.fish.weights.evade->getRealValue()/weightSum);
	}

	force += _steering.wander(this, dt, PM.fish.wander.jitter->getRealValue(), PM.fish.wander.radius->getRealValue(), PM.fish.wander.distance->getRealValue()) 
		* (PM.fish.weights.wander->getRealValue()/weightSum);

	Simulation::Get().tagNeighborAgents(*this, PM.fish.flocking.neighborRadius->getRealValue(), AGENT_FISH);
	force += _steering.alignment(this) * (PM.fish.weights.alignment->getRealValue()/weightSum);
	force += _steering.cohesion(this, getMaxVelocity()) * (PM.fish.weights.cohesion->getRealValue()/weightSum);

	Simulation::Get().tagNeighborAgents(*this, PM.fish.separation.radius->getRealValue(), AGENT_FISH);
	vec3 sepForce = _steering.separation(this);
	if (toDiverVec.squaredLength() < PM.diver.ranges.separation->getRealValue()*PM.diver.ranges.separation->getRealValue())
	{
		sepForce += _steering.separateAgent(this, ArtificialSea::getSingleton().getDiver());
		sepForce.normalise();
	}
	force += sepForce * (PM.fish.weights.separation->getRealValue()/weightSum);

	force = force.normalisedCopy() * PM.forceScale->getRealValue();
	return force;
}

//////////////////////////////////////////////////////////////////////////
	
void Fish::draw() const
{
	//static const float WIDTH = 6.0f;
	//static const float LENGTH = 10.0f;
	//
	//vec2 center(_position.x, _position.y);
	////vec2 heading(_heading.x, _heading.y);
	//vec2 heading(_headingSmoothed.x, _headingSmoothed.y);
	//vec2 side = heading.perpendicular();
	//
	//vec2 baseA = center - side * (WIDTH*0.5f) - heading * (LENGTH*0.5f);
	//vec2 baseB = center + side * (WIDTH*0.5f) - heading * (LENGTH*0.5f);
	//vec2 front = center + heading * (LENGTH*0.5f);
	//
	//VideoDriver::Get().DrawFilledTriangle(
	//	front.x, front.y, baseA.x, baseA.y, baseB.x, baseB.y, Colors::LightCoral);
	//VideoDriver::Get().DrawHollowTriangle(
	//	front.x, front.y, baseA.x, baseA.y, baseB.x, baseB.y, 2, Colors::Firebrick);
}

void Fish::update( float dt )
{
	Agent::update(dt);

	_animationState->addTime(dt);

	//_sceneNode->setPosition(_position);
	//_sceneNode->setDirection(_headingSmoothed, Ogre::SceneNode::TS_WORLD, vec3::NEGATIVE_UNIT_X);

	vec3 xAxis = -_headingSmoothed;
	vec3 zAxis = xAxis.crossProduct(vec3(0,1,0)).normalisedCopy();
	vec3 yAxis = zAxis.crossProduct(xAxis).normalisedCopy();

	_sceneNode->resetOrientation();
	_sceneNode->setPosition(_position);
	_sceneNode->setOrientation(quat(xAxis, yAxis, zAxis));

	//_sceneNode->roll(-_sceneNode->getOrientation().getRoll(false), Ogre::Node::TS_LOCAL);

	//vec3 xVec(-_headingSmoothed);
	//vec3 zVec = xVec.crossProduct(vec3(0,1,0));
	//vec3 yVec = xVec.crossProduct(zVec);
	//quat orient(xVec, yVec, zVec);
	//_sceneNode->resetOrientation();
	//_sceneNode->setOrientation(orient);

	//vec3 localY = _sceneNode->getOrientation() * vec3::UNIT_Y;
	//quat q = localY.getRotationTo(vec3::UNIT_Y);
	//_sceneNode->rotate(q, Ogre::Node::TS_WORLD);
}

//////////////////////////////////////////////////////////////////////////